#include "Joint.h"
#include "Skeleton.h"

/**
* @brief For tracking memory leaks under windows using the crtdbg
*/
#if ( defined( _DEBUG ) || defined( DEBUG ) ) && defined( _MSC_VER )
#define _CRTDBG_MAP_ALLOC
#include <crtdbg.h>

#define DEBUG_NEW new( _NORMAL_BLOCK, __FILE__, __LINE__ )
#define new DEBUG_NEW
#endif

namespace Etoile
{


	Joint::Joint(const std::string& name, Skeleton* skeleton, int id , int parentId)
		: _name(name), _id(id),
		_parent(parentId),_pSkeleton(skeleton), _length(0)
	{
	}

	Joint* Joint::getParent() const
	{
		if(_parent == -1)
		{
			return NULL;
		}
		assert(_pSkeleton != NULL);
		return _pSkeleton->getJoint(_parent);
	}

	Skeleton* Joint::getSkeleton() const
	{ 
		return _pSkeleton;
	}

	void Joint::setSkeleton(Skeleton* skeleton)
	{ 
		_pSkeleton = skeleton;
	}

	bool Joint::addChild(int id)
	{
		if(_pSkeleton == NULL)
		{
			return false;
		}

		Joint* joint = _pSkeleton->getJoint(id);

		if(joint != NULL)
		{
			_children.push_back(id);

			return true;
		}
		return false;
	}

	bool Joint::addChild(Joint* joint)
	{
		if(_pSkeleton == NULL)
		{
			return false;
		}

		joint->setParent(_id);
		int added = _pSkeleton->addJoint(joint, _id);
		if(added > 0)
		{
			_children.push_back(joint->getId());
		}

		return true;
	}

	Joint* Joint::getChild(unsigned int index)
	{
		if(index < _children.size())
			return _pSkeleton->getJoint(_children[index]);
		return 0;
	}


	bool Joint::removeChild(unsigned int index)
	{
		if(_pSkeleton == NULL || index >= _children.size())
		{
			return false;
		}

		bool rem = _pSkeleton->removeJoint(_children[index]);
		if(rem)
			_children.erase(_children.begin() + index);
		return rem;
	}


	void Joint::rotate(const Quaternion& rotation)
	{
		Quaternion tmp = rotation;
		tmp.normalize();
		_localRotation *= tmp;
	}

	void Joint::updateLocally()
	{
		if(_parent == -1)
		{
			_worldRotation = _localRotation ;
			_worldRotation.normalize();
			_worldPosition = _worldRotation * _localPosition;
		}
		else
		{
			_worldPosition = getParent()->getWorldRotation() * _localPosition;
			_worldPosition += getParent()->getWorldPosition();
			//Given two quaternions p, q and multiplying them to form the quaternion r = pq,
			//applying the quaternion r to a vector v first rotates the vector by q then by p.
			//still wonder which should be first local or global G1 = G0 * L   or G1 =  L * G0
			_worldRotation = getParent()->getWorldRotation() * _localRotation;
			_worldRotation.normalize();
		}
	}

	void Joint::update()
	{
		if(_parent == -1)
		{
			_worldRotation = _localRotation ;
			_worldRotation.normalize();
			_worldPosition = _worldRotation * _localPosition;
		}
		else
		{
			_worldPosition = getParent()->getWorldRotation() * _localPosition;
			_worldPosition += getParent()->getWorldPosition();
			//Given two quaternions p, q and multiplying them to form the quaternion r = pq,
			//applying the quaternion r to a vector v first rotates the vector by q then by p.
			//still wonder which should be first local or global G1 = G0 * L   or G1 =  L * G0
			_worldRotation = getParent()->getWorldRotation() * _localRotation;
			_worldRotation.normalize();
		}

		for (std::vector<int>::iterator it = _children.begin(); it != _children.end(); ++it)
		{
			_pSkeleton->getJoint((*it))->update();
		}
	}

	void Joint::reset()
	{
		_localRotation = Quaternion();
		updateLocally();
		for (std::vector<int>::iterator it = _children.begin(); it != _children.end(); ++it)
		{
			_pSkeleton->getJoint((*it))->reset();
		}
	}

	void Joint::setOriginalLocalPosition(const Vec3d& pos)
	{
		_localPosition = pos;
		_originalLocalPosition = pos;
		_length = pos.length();
	}

	double Joint::getLength()
	{
		return _length;
	}

	void Joint::setLocalRotation(const Quaternion& local)
	{
		_localRotation = local;
	}

	Quaternion Joint::getLocalRotation()  
	{
		return _localRotation;
	}

	/*void Joint::setWorldRotation(const Quaternion& world)
	{
		_worldRotation = world;
	}*/

	Quaternion Joint::getWorldRotation()  
	{
		return _worldRotation;
	}

	void Joint::setLocalPosition(const Vec3d& pos)
	{
		_localPosition = pos;
	}

	const Vec3d& Joint::getLocalPosition() 
	{
		return _localPosition;
	}

	/*void Joint::setWorldPosition(const Vec3d& pos)
	{
		_worldPosition = pos;
	}*/

	const Vec3d& Joint::getWorldPosition()  
	{
		return _worldPosition;
	}

}
